﻿using System.Collections.Generic;
using BulletSharp;
using System;
using btl.generic;

namespace Robot
{
    class RobotModel
    {
        Vector3 vUp = new Vector3(0, 1, 0);
        public static int NumLegs = 6;

        List<Leg> legs = new List<Leg>();
        DynamicsWorld _world;

        const float depth = 0.5f;
        const float fBodySize = 0.25f;

        CollisionShape shape;
        RigidBody body;
        MotionState initialMotionState;

        Food food;

        Matrix startTransform = Matrix.Translation(new Vector3(0, 2, 0));

        //Genetic
        public double Fitness { get; set; }
        public double Distance { get; set; }
        Genome _genome;
        public Genome Genome
        {
            get
            {
                return _genome;
            }
            set
            {
                _genome = value;
                if (value != null)
                {
                    SetWeights(value.Genes());
                }
            }
        }

        public void evaluate()
        {
            Vector3 pos1 = body.WorldTransform.Origin;
            Vector3 pos2 = food.body.WorldTransform.Origin;
            Distance = Vector2.Distance(new Vector2(pos1.X, pos1.Z), new Vector2(pos2.X, pos2.Z));
            Fitness = 1 / Distance;
        }

        public static RigidBody LocalCreateRigidBody(float mass, DynamicsWorld world, Matrix startTransform, CollisionShape shape)
        {
            //rigidbody is dynamic if and only if mass is non zero, otherwise static
            bool isDynamic = (mass != 0.0f);

            Vector3 localInertia = Vector3.Zero;
            if (isDynamic)
                shape.CalculateLocalInertia(mass, out localInertia);

            //using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
            DefaultMotionState myMotionState = new DefaultMotionState(startTransform);
            RigidBodyConstructionInfo rbInfo = new RigidBodyConstructionInfo(mass, myMotionState, shape, localInertia);
            RigidBody body = new RigidBody(rbInfo);
            rbInfo.Dispose();

            world.AddRigidBody(body);

            return body;
        }

        public RobotModel(DynamicsWorld world, Vector3 position, Food food)
        {
            this.food = food;
            _world = world;
            const float fBodySize = 0.25f;

            shape = new CapsuleShape(fBodySize, 0.10f);

            //
            // Setup rigid bodies
            //
            const float fHeight = 0.5f;
            Matrix offset = Matrix.Translation(position);

            // root
            Vector3 vRoot = new Vector3(0, fHeight, 0);
            Matrix transform = Matrix.Translation(vRoot);

            body = LocalCreateRigidBody(1, world, transform * offset, shape);
            initialMotionState = body.MotionState;

            body.SetDamping(0.05f, 0.85f);
            body.DeactivationTime = 0.8f;
            //bodies[i].SetSleepingThresholds(1.6f, 2.5f);
            body.SetSleepingThresholds(0.5f, 0.5f);


            for (int i = 0; i < NumLegs; i++)
            {
                float fAngle = (float)(2 * Math.PI * i / NumLegs);
                legs.Add(new Leg(world, body, depth, fHeight, offset, fAngle));
            }

            
        }

        public void Update(float timeStep)
        {
            foreach (Leg leg in legs)
            {
                leg.Update(timeStep);
            }
        }

        public static RigidBody CreateRigidBody(float mass, Matrix startTransform, CollisionShape shape)
        {
            //rigidbody is dynamic if and only if mass is non zero, otherwise static
            bool isDynamic = (mass != 0.0f);

            Vector3 localInertia = Vector3.Zero;
            if (isDynamic)
                shape.CalculateLocalInertia(mass, out localInertia);

            MotionState ms = new DefaultMotionState(startTransform);
            RigidBodyConstructionInfo rbInfo = new RigidBodyConstructionInfo(mass, ms, shape, localInertia);
            //rbInfo.StartWorldTransform = startTransform;
            RigidBody body = new RigidBody(rbInfo);
            rbInfo.Dispose();

            //body.ActivationState = ActivationState.DisableDeactivation;

            // Constrain z-movement and xy-rotation
            body.LinearFactor = new Vector3(1, 1, 0);
            body.AngularFactor = new Vector3(0, 0, 1);

            return body;
        }

        public void Remove(DynamicsWorld world)
        {
            world.RemoveRigidBody(body);
            foreach (Leg leg in legs)
            {
                leg.Remove(world);
            }
        }

        void SetWeights(double[] weights)
        {
            int i=0;
            foreach (Leg leg in legs)
            {
                leg.period = (float)weights[i++];
                leg.period2 = (float)weights[i++];
            }
        }
    }
}
